#include <cv.h>
#include <highgui.h>
#include <cxcore.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>

#include <iostream>
#include <iomanip>
#include <vector>

#include <sstream>
#include <string>

using namespace std;

#include "svm.h"
#include "svmpredict.h"

//#include "sift.h"
//#include "imgfeatures.h"
//#include "utils.h"
#include "ImageManip.h"

#define PI 3.14159265

template<class T> class Image
{
private:
  IplImage* imgp;
public:
  Image(IplImage* img=0) {imgp=img;}
  ~Image(){imgp=0;}
  void operator=(IplImage* img) {imgp=img;}
  inline T* operator[](const int rowIndx) {
    return ((T *)(imgp->imageData + rowIndx*imgp->widthStep));}
};

typedef struct{
  unsigned char b,g,r;
} RgbPixel;

typedef struct{
  float b,g,r;
} RgbPixelFloat;

typedef struct{
  double x,y,majorAxis,minorAxis,angle;
  double f1_x,f1_y,f2_x,f2_y;
} Ellipse;

typedef Image<RgbPixel>       RgbImage;
typedef Image<RgbPixelFloat>  RgbImageFloat;
typedef Image<unsigned char>  BwImage;
typedef Image<float>          BwImageFloat;

typedef struct{
  int id;
  CvConnectedComp comp;
} track;

struct svm_model* model2;

// Global Variable
IplConvKernel* myKernel =  cvCreateStructuringElementEx( 30, 30, 15, 15, CV_SHAPE_ELLIPSE );
static CvScalar colors[] = {
  {{0,0,255}},
  {{0,128,255}},
  {{0,255,255}},
  {{0,255,0}},
  {{255,128,0}},
  {{255,255,0}},
  {{255,0,0}},
  {{255,0,255}},
  {{255,255,255}}
};



// Function Prototype
CvSURFParams cvSURFParams(double threshold, int extended);
double acot_d(double val);
bool pointInEllipse(CvPoint point,Ellipse ellipse);
void getEllipseParam(double a,double b,double c,double d,double f,double g,Ellipse& ellipse);
void myMatPrint( ostream &outStream, string stringLabel, CvMat &mat );
Ellipse fitEllipseRANSAC(vector<CvPoint> points,int &count);
int detectionTracking(IplImage* object, IplImage* colorImage, vector<track>* vtrackStore, int use_harris, 
		      char* saveFileImagName, int maxID, int frameNo, char* outFileDetailName, char* outFileIDName1);



// Main
int main( int argc, char** argv )
{
  //declare for detectionTracking
  //int count=1;
  double time1;
		
  CvCapture* capture;
  int callVideo = 0;
  IplImage* frame;
  IplImage* frame2;
  vector<track> vtrackStore;
  //int direction;


  //Feature and Descriptor Extraction
  char featOpt[] = "SURF"; //SIFT: SIFT, HSIFT: Harris+SIFT, SURF: SURF, HSURF: Harris+SURF

  //Parameter setting
  char outFoldRoot[]="Row3_LeftBack_out";	//output directory
  char image_name[] = "Row3_LeftBack_pic/pic_"; //caputured imges from video, call captured images instead of call video directly
  char image_ext[] = ".png"; //image extension
  int iImageCountS = 201; //start frame
  int iImageCountE = 1700; //end frame
  char modeltxt[] = "svmModel/SURF_model.txt";	//svm model

  int num_images = iImageCountE-iImageCountS+1; //number of total frame
  char szImageFileName[100];  //input images
  char saveFileImagName[100]; //output images
		
  //IplImage* pImage;
  //struct feature* features;
  //int n = 0;

  //save features and descriptors
  //char szSiftDescrFileName1[100];
  //char szSiftDescrFileName2[100];
  //char szImageWithSiftFeaturesName[100];
	
  char outFold[100];
  //char outFile[100];
  char strCommand[100];
	
  ///harris parameters
  int use_harris = 0;
  //int blockSizeHarris=3;
  //int maskSizeHarris=3;
  //double kappaHarris=0.235;	//=0.235 HSIFT return 1010 points
  //double kappaHarris=0.23; //=0.23 HSURF return 1003 points
	
  ///SIFT and HSIFT parameters
  //int intvls = 3; //SIFT_INTVLS; //=3

  //double contr_thr = 0.04; //SIFT_CONTR_THR;  //=0.04
  //int curv_thr = 10; //SIFT_CURV_THR; //=10 calculate every 10 degree to consider the gradient is big or not

  //int descr_width = 4; //SIFT_DESCR_WIDTH; //=4
  //int descr_hist_bins = 8; //SIFT_DESCR_HIST_BINS; //=8

  //double sigma = 1.6; //SIFT_SIGMA; //=1.6
  //int img_dbl = 0; //SIFT_IMG_DBL; //=0
	
  double tt;	
	
  //load SVM model	
  //char* modeltxt = "svmModel/SURF_model.txt";		
  double time2 = (double)cvGetTickCount();		
  model2 = loadModel(modeltxt);
  time2 = (double)cvGetTickCount() - time2;
  cout << "model file: " << modeltxt << endl;
  cout << "Time svm load model: " << time2/(cvGetTickFrequency()*1000.) << " ms" << endl; 
	
  ///uncomment these lines for calling video directly
  //capture = cvCreateFileCapture( "Row3_LeftBack.wmv" ); //captured video
  //callVideo = 1;
  //for(int i=1; i<iImageCountS; i++){
  //	frame = cvQueryFrame( capture );
  //}


  char outFileDetailName[100];
  char outFileIDName[100];
  //FILE *outFileDetail;
  //FILE *outFileID;
  int maxID = -1;	
  for( int iImageCount = iImageCountS; iImageCount <= iImageCountE; iImageCount++ )
    {
      //write text file output for Ice's defence
      sprintf(outFold, "%s/%d", outFoldRoot, iImageCount);
      sprintf(strCommand, "mkdir %s", outFold);
      system(strCommand);
		
      sprintf(outFileDetailName, "%s/trackedDetail.txt", outFold);
      sprintf(outFileIDName, "%s/", outFold);
		
      //outFileDetail = fopen(outFileDetailName, 'a');
      //outFileID = fopen(outFileIDName, 'w');
		
      time1 = (double)cvGetTickCount();

      //call input video or input images
      //call input video
      capture = NULL;
      if(callVideo){
	frame = cvQueryFrame( capture ); //frame of capture
	frame2 = cvCreateImage(cvSize(frame->width,frame->height),8,1);
	cvCvtColor(frame,frame2,CV_BGR2GRAY);

      }else{		//call input images		
	if( num_images > 1 )	{
	  if(iImageCount < 10){
	    sprintf(szImageFileName, "%s000%d%s", image_name, iImageCount, image_ext );
	  }else if(iImageCount < 100){
	    sprintf(szImageFileName, "%s00%d%s", image_name, iImageCount, image_ext );
	  }else if(iImageCount < 1000){
	    sprintf(szImageFileName, "%s0%d%s", image_name, iImageCount, image_ext );
	  }else{
	    sprintf(szImageFileName, "%s%d%s", image_name, iImageCount, image_ext );
	  }
	}
	else {
	  sprintf(szImageFileName,"%s%s", image_name, image_ext ); 
	}

	frame = cvLoadImage( szImageFileName, CV_LOAD_IMAGE_COLOR ) ;
	frame2 = cvCreateImage(cvSize(frame->width,frame->height),8,1);
	cvCvtColor(frame,frame2,CV_BGR2GRAY);

      }
	
      //save output images
      if( num_images > 1 )	{
	if(iImageCount < 10){
	  sprintf(saveFileImagName, "%s/out_000%d%s", outFoldRoot, iImageCount, image_ext );
	}else if(iImageCount < 100){
	  sprintf(saveFileImagName, "%s/out_00%d%s", outFoldRoot, iImageCount, image_ext );
	}else if(iImageCount < 1000){
	  sprintf(saveFileImagName, "%s/out_0%d%s", outFoldRoot, iImageCount, image_ext );
	}else{
	  sprintf(saveFileImagName, "%s/out_%d%s", outFoldRoot, iImageCount, image_ext );
	}
      }
      else {
	sprintf(saveFileImagName, "%s/out_%d%s", outFoldRoot, iImageCount, image_ext );
      }
		
      //if( pImage ){	
      if( frame ){
	// SURF //
	if(!strcmp(featOpt,"SURF")){
	  //IplImage* object = cvLoadImage( szImageFileName, CV_LOAD_IMAGE_GRAYSCALE );
	  //CvSeq *objectKeypoints = 0;
	  CvSeq *objectDescriptors = 0;
	  CvMemStorage* storage = cvCreateMemStorage(0);
	  CvSURFParams params = cvSURFParams(500, 1);
	  CvSeq *keypointSeq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSURFPoint), storage );
				
	  tt = (double)cvGetTickCount();
	  //cvExtractSURF( object, 0, &keypointSeq, &objectDescriptors, storage, params,0); //0:onlySURF 1:Harris+SURF
	  cvExtractSURF( frame2, 0, &keypointSeq, &objectDescriptors, storage, params,0); //0:onlySURF 1:Harris+SURF
	  tt = (double)cvGetTickCount() - tt;
	  //cout << "keypointSeq->total: " << keypointSeq->total << endl;
	}
      }else{
	cout << "Unable to load image from " << szImageFileName << endl;			
      }
				
      //printf( "Time Feature Descriptor Extration = %gms\n", tt/(cvGetTickFrequency()*1000.));
      //cout << endl << featOpt << endl;

      //End Feature and Descriptor Extraction

      //frame = (struct _IplImage*) cvClone( pImage );
		
      //if( !frame ) break;
			
      //frame2 = cvCreateImage(cvSize(frame->width,frame->height),8,1);
      //cvCvtColor(frame,frame2,CV_BGR2GRAY);
      //direction = 1 go from top to bottom, = 2 go from bottom to top
      //direction = 1;
      //frame = BGR image
      //frame2 = Gray image


      maxID = detectionTracking(frame2, frame, &vtrackStore, use_harris, saveFileImagName, maxID, iImageCount, outFileDetailName, outFileIDName);	
      time1 = (double)cvGetTickCount() - time1;
      cout << endl << "Time of frame " << iImageCount << ": " << time1/(cvGetTickFrequency()*1000.) << " ms" << endl << endl;
    
    
      char c = cvWaitKey(33);
      if( c == 27 ) break;
	
      //End Tracking
    } //End for( int iImageCount = (iImageCountS-1); iImageCount < iImageCountE; iImageCount++ )

  //cout << endl << "Number of total ID: " << (vtrackStore).size() << endl;
	
  //vtrackStore.clear();
  cout << endl << "Number of total ID: " << maxID << endl;

  cvReleaseImage(&frame);
  cvReleaseImage(&frame2);
		
  cout << "Press ENTER to exit..." << endl;
  std::cin.get();
  return 0;
}


/*****************/
/*   Functions   */
/*****************/

CvSURFParams cvSURFParams(double threshold, int extended){
  CvSURFParams params;
  params.hessianThreshold = threshold;
  params.extended = extended;
  params.nOctaves = 1;//4;
  params.nOctaveLayers = 2;
  return params;
}

double acot_d(double val){
  //double at = atan(val);
  //cout << "atan : " << at << endl;
  double acot = atan(1/val);
    
  return acot*180/PI;
}

bool pointInEllipse(CvPoint point,Ellipse ellipse){
  double dist1 = sqrt((point.x - ellipse.f1_x) * (point.x - ellipse.f1_x) + 
		      (point.y - ellipse.f1_y) * (point.y - ellipse.f1_y));
  double dist2 = sqrt((point.x - ellipse.f2_x) * (point.x - ellipse.f2_x) + 
		      (point.y - ellipse.f2_y) * (point.y - ellipse.f2_y));
  //cout << "dist : " << dist1 << " " << dist2 << endl;
  //cout << "total dist : " << dist1+dist2 << " 2*a : " << 2*ellipse.majorAxis << endl;
  double max;
  if(ellipse.majorAxis > ellipse.minorAxis)
    max = ellipse.majorAxis;
  else
    max = ellipse.minorAxis;
  if(dist1+dist2 <= 2*max)
    return true;
  else
    return false;
}

void getEllipseParam(double a,double b,double c,double d,double f,double g,Ellipse& ellipse){
  //		     double &x,double &y,double &majorAxis,double &minorAxis,double &angle){
  ellipse.x = (c * d - b * f)/(b * b - a * c);
  ellipse.y = (a * f - b * d)/(b * b - a * c);
  
  ellipse.majorAxis = sqrt( (2*(a*f*f+c*d*d+g*b*b-2*b*d*f-a*c*g))/((b*b-a*c)*(sqrt((a-c)*(a-c)+4*b*b)-(a+c))));
  ellipse.minorAxis = sqrt( (2*(a*f*f+c*d*d+g*b*b-2*b*d*f-a*c*g))/((b*b-a*c)*(sqrt((a-c)*(a-c)+4*b*b)+(a+c))));
  //cout << "numerator : " << (2*(a*f*f+c*d*d+g*b*b-2*b*d*f-a*c*g)) << endl;
  //cout << "denominator: " << ((b*b-a*c)*(sqrt((a-c)*(a-c)+4*b*b)-(a+c))) << endl;
  //cout << "denominator: " << ((b*b-a*c)*(-1*sqrt((a-c)*(a-c)+4*b*b)-(a+c))) << endl;
  cout << "x : " << ellipse.x << endl;
  cout << "y : " << ellipse.y << endl;
  cout << "majorAxis : " << ellipse.majorAxis << endl;
  cout << "minorAxis : " << ellipse.minorAxis << endl;


  ellipse.angle=0;
  if(b == 0 && a < c){
    cout << "case 1" << endl;
    ellipse.angle = 0;
  }
  else if(b == 0 && a > c){
    cout << "case 2" << endl;
    ellipse.angle = 90;
  }
  else if(b != 0 && a < c){
    cout << "case 3" << endl;
    //angle = 1/2 * atan(   (a-c)/(2*b)  );
    //angle = 0.5 * atan( (2*b)/(a-c)  )*180/PI;
    ellipse.angle = 0.5 * acot_d( (a-c)/(2*b) );
  }
  else if(b != 0 && a > c){
    cout << "case 4" << endl;
    //angle = 90 + 1/2 * atan( (a-c)/(2*b)  );
    //angle = 90 + 0.5 * atan( (2*b)/(a-c)  )*180/PI;
    ellipse.angle = 90 + 0.5 * acot_d( (a-c)/(2*b) );
  }
  cout << "angle : " << ellipse.angle << endl;
  //cout << "atan : " << atan( (2*b)/(a-c))*180/PI << endl;
  if(ellipse.minorAxis > ellipse.majorAxis){
    double temp = ellipse.majorAxis;
    ellipse.majorAxis = ellipse.minorAxis;
    ellipse.minorAxis = temp;
    ellipse.angle += 90;
  }

  double temp_c;
  if(ellipse.majorAxis > ellipse.minorAxis)
    temp_c = sqrt(ellipse.majorAxis * ellipse.majorAxis - ellipse.minorAxis * ellipse.minorAxis);
  else
    temp_c = sqrt(ellipse.minorAxis * ellipse.minorAxis - ellipse.majorAxis * ellipse.majorAxis);
  ellipse.f1_x = ellipse.x - temp_c * cos(ellipse.angle*PI/180);
  ellipse.f1_y = ellipse.y - temp_c * sin(ellipse.angle*PI/180);
  ellipse.f2_x = ellipse.x + temp_c * cos(ellipse.angle*PI/180);
  ellipse.f2_y = ellipse.y + temp_c * sin(ellipse.angle*PI/180);
  cout << "foci : (" << ellipse.f1_x << "," << ellipse.f1_y << ") (" << ellipse.f2_x << "," << ellipse.f2_y << ")" << endl;
}

void myMatPrint( ostream &outStream, string stringLabel, CvMat &mat )
{
    outStream << stringLabel << endl;
    for (int iRow = 0; iRow < mat.rows; iRow++)
    {
        for (int iCol = 0; iCol < mat.cols; iCol++)
        {
	  outStream << setw(12) << cvmGet(&mat,iRow,iCol);
        }
        outStream << endl;
    }
}

Ellipse fitEllipseRANSAC(vector<CvPoint> points,int &count){
  Ellipse ellipse;
  count=0;
  int index[5];
  bool match=false;
  for(int i=0;i<5;i++){
    do {
      match = false;
      index[i]=rand()%points.size();
      for(int j=0;j<i;j++){
	if(index[i] == index[j]){
	  match=true;
	}
      }
    }
    while(match);
  }
  double aData[] = {
    points[index[0]].x * points[index[0]].x, 2 * points[index[0]].x * points[index[0]].y, points[index[0]].
    y * points[index[0]].y, 2 * points[index[0]].x, 2 * points[index[0]].y,

    points[index[1]].x * points[index[1]].x, 2 * points[index[1]].x * points[index[1]].y, points[index[1]].
    y * points[index[1]].y, 2 * points[index[1]].x, 2 * points[index[1]].y,

    points[index[2]].x * points[index[2]].x, 2 * points[index[2]].x * points[index[2]].y, points[index[2]].
    y * points[index[2]].y, 2 * points[index[2]].x, 2 * points[index[2]].y,

    points[index[3]].x * points[index[3]].x, 2 * points[index[3]].x * points[index[3]].y, points[index[3]].
    y * points[index[3]].y, 2 * points[index[3]].x, 2 * points[index[3]].y,

    points[index[4]].x * points[index[4]].x, 2 * points[index[4]].x * points[index[4]].y, points[index[4]].
    y * points[index[4]].y, 2 * points[index[4]].x, 2 * points[index[4]].y };
  CvMat matA=cvMat(5,5,CV_64F,aData);
  CvMat *D,*U,*V;
  D=cvCreateMat(5,5,CV_64F);
  U=cvCreateMat(5,5,CV_64F);
  V=cvCreateMat(5,5,CV_64F);
  
  cvSVD(&matA,D,U,V,CV_SVD_MODIFY_A);
  
  double a,b,c,d,f,g;
  a=cvmGet(V,0,4);
  b=cvmGet(V,1,4);
  c=cvmGet(V,2,4);
  d=cvmGet(V,3,4);
  f=cvmGet(V,4,4);
  g=1;
  //cout << a << " " << b << " " << c << " " << d << " " << f << " " << g << endl;
  getEllipseParam(a,b,c,d,f,g,ellipse);
  //cvZero( ellipse_image );
  //cvDrawContours(ellipse_image , contour, colors[0], colors[0], -1, 1, 8 );
  vector<CvPoint>::iterator point_iter;
  if(ellipse.majorAxis > 0 && ellipse.minorAxis > 0){
    for(point_iter=points.begin();point_iter!=points.end();point_iter++){
      CvPoint point = *point_iter;
      if(pointInEllipse(point,ellipse)){
	count++;
	//cvCircle( ellipse_image, point, 1, colors[2], 1, 8, 0 );
      }
      else{
	//cvCircle( ellipse_image, point, 1, colors[4], 1, 8, 0 );
      }
    }
    cout << "count : " << count << endl;
  }
  
  //if(ellipse.majorAxis > 0 && ellipse.minorAxis > 0)
  //  cvEllipse(ellipse_image,cvPoint(ellipse.x,ellipse.y),cvSize(ellipse.majorAxis,ellipse.minorAxis),ellipse.angle,0,360,colors[0]);
  
  //cvNamedWindow( "Ellipse", 1 );
  //cvMoveWindow( "Ellipse",0,0);
  //cvShowImage( "Ellipse", ellipse_image );
  //cvWaitKey(33);
  //if( c == 27 ) break;
  return ellipse;
}


////////////////////////////////////////////////////////////////////////////////////////////////
int detectionTracking(IplImage* object, IplImage* colorImage, vector<track>* vtrackStore, int use_harris, 
		      char* saveFileImagName, int maxID, int frameNo, char* outFileDetailName, char* outFileIDName1)
{
  static int id=0;
  id++;
  //int n = 0;
  //char object_filename[100];
  //int iImageNum;
  //int i;
  char outFileIDName2[100];


  CvMemStorage* storage = cvCreateMemStorage(0);


  CvSeq *keypointSeq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvSURFPoint), storage );
  CvSeq *objectKeypoints = 0, *objectDescriptors = 0;
  CvSURFParams params = cvSURFParams(500, 1);
  cvExtractSURF( object, 0, &keypointSeq, &objectDescriptors, storage, params,use_harris );
    
  //tt = (double)cvGetTickCount() - tt;
  //printf( "First image Extraction time = %g ms\n", tt/(cvGetTickFrequency()*1000.));
  //printf("after cvExtractSURF \n");

  ///Ice add: to show features of SURF///////////////////////////////////////////
  /*IplImage* object_color = cvCreateImage(cvGetSize(object), 8, 3);
  cvCvtColor( object, object_color, CV_GRAY2BGR );
  cvNamedWindow("Features",1);
  cvMoveWindow("Features",0,0);

  for( i = 0; i < keypointSeq->total; i++ )
  {
     CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( keypointSeq, i );
     //printf("(%f,%f) ",r->pt.x,r->pt.y);
     CvPoint center;
     int radius;
     center.x = cvRound(r->pt.x);
     center.y = cvRound(r->pt.y);
     //radius = cvRound(r->size*1.2/9.*2);
     radius = 1;
     cvCircle( object_color, center, radius, colors[0], 1, 8, 0 );
  }
  cvShowImage( "Features", object_color );
  cvSaveImage("features.png",object_color);
  cvReleaseImage( &object_color );*/
  
  //////////////////////////////////////////////////////
  //printf("Object Descriptors: %d \n", objectDescriptors->total);
  ///////////////// Write descriptor to file :: added by ice /////////////////
  //writeDescriptorsToFile1(keypointSeq,objectDescriptors,szDescrFileName1,szFrameFileName1);
  /**  **/
  //	writeDescriptorsToFile1Label(keypointSeq,objectDescriptors,szDescrFileName1,szFrameFileName1);

  CvSeqReader reader;
  cvStartReadSeq( objectDescriptors, &reader );
  //const float* descriptor = (const float*)reader.ptr;
	

  /** Do not write descriptors to txt file and use it as a variable to predict2 function **/
  //	writeDescriptorsToFile1Label(keypointSeq,objectDescriptors,szDescrFileName1,szFrameFileName1);

  //int descriptorLength = 128;
  //int objKeypointCount = (int)(objectDescriptors->elem_size/sizeof(float));
  int objKeypointCount = objectDescriptors->total;
  int predicted_val_arr[objKeypointCount];
	
  //double time1 = (double)cvGetTickCount();
  predict2(&objectKeypoints,&objectDescriptors,128,objKeypointCount,model2,predicted_val_arr); 
  //time1 = (double)cvGetTickCount() - time1;
  //cout << endl << "Time svm predict: " << time1/(cvGetTickFrequency()*1000.) << " ms" << endl;
	

  int predicted_val = 0;
  IplImage* pImageSVM = cvCreateImage( cvSize(object->width,object->height), 8, 1 );
  IplImage* pImageSVM_color = cvCreateImage( cvSize(object->width,object->height), 8, 3 );
  cvCvtColor( object, pImageSVM_color, CV_GRAY2BGR );
  //Make every pixels of pImageSVM black 
  BwImage imgSVMWrapper( pImageSVM );
  for( int yy = 0; yy < (pImageSVM->height); ++yy ) {
    for( int xx = 0; xx < (pImageSVM->width); ++xx ) {
      imgSVMWrapper[yy][xx]=0;
    }
  }
	
  //cvNamedWindow("pImageSVM 2", 1);
  //cvMoveWindow("pImageSVM 2",640,0);
  //cvShowImage("pImageSVM 2", pImageSVM);	
  vector<CvPoint> pineapplePoints;

  for( int i = 0; i < objKeypointCount; i++ )
    {
      CvSURFPoint* r = (CvSURFPoint*)cvGetSeqElem( keypointSeq, i );
      CvPoint center;
      center.x = cvRound(r->pt.x);
      center.y = cvRound(r->pt.y);
      predicted_val = predicted_val_arr[i];
       
      if( predicted_val == 1 ) {
	imgSVMWrapper[center.y][center.x] = 255;
	cvCircle( pImageSVM_color, center, 1, colors[0], 1, 8, 0 );
	pineapplePoints.push_back(center);
      }
    }
  //cvNamedWindow("Pineapple point", 1);
  //cvMoveWindow("Pineapple point",640,0);
  //cvShowImage("Pineapple point", pImageSVM_color);
  //cvSaveImage("pinepoint.png", pImageSVM_color);

  
  cvMorphologyEx( pImageSVM, pImageSVM, NULL, myKernel, CV_MOP_CLOSE, 1 );
  //cvNamedWindow( "Closing result", 1 );
  //cvMoveWindow("Closing result",640,480);
  //cvShowImage( "Closing result", pImageSVM );
  //cvReleaseImage( &pImageSVM );
	
	
  //BwImage imgSVMWrapper( pImageSVM );
	
  IplImage* pImageBlob = cvCloneImage( object );
  int height = (int)pImageSVM->height;
  int width = pImageSVM->width;
  vector<CvConnectedComp> vcomp;
  CvPoint xy;
  int MIN_REGION_SIZE = 700;
    
  //double time1 = (double)cvGetTickCount();
  //
  for( int y = 0; y < height; ++y ) {
    for( int x = 0; x < width; ++x ) {
      if( imgSVMWrapper[y][x] > 0 ) {
	CvConnectedComp comp;
	xy.x = x;
	xy.y = y;
	cvFloodFill( pImageSVM, xy, cvScalarAll( 255 ), cvScalarAll( 0 ), cvScalarAll( 0 ), &comp );
      }
    }
  }  
  
  //direction = 1 go from top to bottom, = 2 go from bottom to top
  //for( int y = 0; y < height; ++y ) {
  //	for( int x = 0; x < width; ++x ) {

  for( int y = (height-1); y >= 0; --y ) {
    for( int x = (width-1); x >= 0 ; --x ) {
      if( imgSVMWrapper[y][x] == 255 ) {
	CvConnectedComp comp;
	xy.x = x;
	xy.y = y;
	cvFloodFill( pImageSVM, xy, cvScalarAll( 254 ), cvScalarAll( 0 ), cvScalarAll( 0 ), &comp );
	if( ( int )comp.area > MIN_REGION_SIZE ) {                	
	  vcomp.push_back( comp );
	}
      }
    }
  }
    
        
  //cout << "Number of this frame detected blob: " << vcomp.size() << endl;
  //cvNamedWindow( "Closing", 1 );
  //cvMoveWindow( "Closing",640,480);
  //cvShowImage( "Closing", pImageSVM );
  //cvSaveImage("closing.png", pImageSVM);

  CvSeq* contour = 0;
  //CvMemStorage* storage = cvCreateMemStorage(0);
  cvFindContours(pImageSVM, storage, &contour, sizeof(CvContour),
		 CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
  CvSeq* first_contour = contour;

  //IplImage* imgContourWithPoints = cvCreateImage( cvGetSize(object), 8, 3 );
  //cvZero( imgContourWithPoints );
  IplImage* imgContourWithPoints = cvCloneImage(colorImage );

  /*vector<CvConnectedComp>::iterator iter;
  for(iter = vcomp.begin(); iter != vcomp.end(); iter++){
    CvScalar rand_color = CV_RGB( rand()&255, rand()&255, rand()&255 );
    cvDrawContours( imgContourWithPoints, iter->contour, rand_color, rand_color, -1, 1, 8 );
    cout << "draw contour size : " << iter->area << endl;
    for(int k=0;k<pineapplePoints.size();k++){
      //cout << "size of connected component : " << comp.area << endl;
      double result=cvPointPolygonTest(iter->contour,cvPointTo32f(pineapplePoints[k]), 0);
      if(result >= 0){
	cvCircle( imgContourWithPoints, pineapplePoints[k], 1, rand_color, 1, 8, 0 );
	cout << "point is inside polygon" << endl;
	//points.push_back(pineapplePoints[k]);
      }
      else{
	cout << "point is outside polygon" << endl;
      }
    }
  }
  cvNamedWindow( "ContourWithPoints", 1 );
  cvMoveWindow( "ContourWithPoints",0,0);
  cvShowImage( "ContourWithPoints", imgContourWithPoints );*/

  vector<vector<CvPoint> > pointsInContours;
  int color_index=0;
  //int ccomp_num=0;
  for( ; contour != 0; contour = contour->h_next ){
    CvScalar rand_color = CV_RGB( rand()&255, rand()&255, rand()&255 );
    //vcomp[ccomp_num].contour = contour;
    //ccomp_num++;
    /* replace CV_FILLED with 1 to see the outlines */
    //cvDrawContours( imgContourWithPoints, contour, rand_color, rand_color, -1, 1, 8 );
    cvDrawContours( imgContourWithPoints, contour, colors[color_index], colors[color_index], -1, 2, 8 );
    vector<CvPoint> points;

    //for(int i=0;i<vcomp.size();i++){
    //CvConnectedComp comp = vcomp[i];
    for(unsigned int k=0;k<pineapplePoints.size();k++){
      //cout << "size of connected component : " << comp.area << endl;
      double result=cvPointPolygonTest(contour,cvPointTo32f(pineapplePoints[k]), 0);
      if(result >= 0){
	//cvCircle( imgContourWithPoints, pineapplePoints[k], 1, rand_color, 1, 8, 0 );
	cvCircle( imgContourWithPoints, pineapplePoints[k], 1, colors[color_index], 1, 8, 0 );
	//cout << "point is inside polygon" << endl;
	points.push_back(pineapplePoints[k]);
      }
      else{
	//cout << "point is outside polygon" << endl;
      }
    }
    color_index++;
    if(color_index==8) color_index=0;
    pointsInContours.push_back(points);
  }

  cvNamedWindow( "ContourWithPoints", 1 );
  cvMoveWindow( "ContourWithPoints",640,0);
  cvShowImage( "ContourWithPoints", imgContourWithPoints );
  
  vector<vector<CvPoint> >::iterator pointsInContours_iter;
  for(pointsInContours_iter = pointsInContours.begin();pointsInContours_iter != pointsInContours.end(); pointsInContours_iter++){
    vector<CvPoint> points = *pointsInContours_iter;
    cout << "points size = " << points.size() << endl;
  }
  //ccomp_num=0;

  //IplImage* ellipse_image = cvCreateImage( cvSize(object->width,object->height), 8, 3 );
  
  for(pointsInContours_iter = pointsInContours.begin(),contour=first_contour;pointsInContours_iter != pointsInContours.end(); pointsInContours_iter++,contour=contour->h_next){
    
    
    vector<CvPoint> points = *pointsInContours_iter;
    
    int count=0;
    int largest_count=0;
    int running_time=0;
    Ellipse largestEllipse;
    //Ellipse ellipse;
    
    if(points.size() >= 10){
      do {
	running_time++;
	if(running_time > 500)
	  break;
	
	Ellipse ellipse = fitEllipseRANSAC(points,count);
	
	if(count > largest_count){
	  largest_count = count;
	  largestEllipse = ellipse;
	}
      }
      while(largest_count < (.60 * points.size()));
      
      if(largestEllipse.majorAxis > 0 && largestEllipse.minorAxis > 0){
	cvEllipse(imgContourWithPoints,cvPoint(largestEllipse.x,largestEllipse.y),
		  cvSize(largestEllipse.majorAxis,largestEllipse.minorAxis),
		  largestEllipse.angle,0,360,colors[0],2);
	/*
	vector<CvPoint> left_points;
	vector<CvPoint>::iterator point_iter;
	for(point_iter=points.begin();point_iter!=points.end();point_iter++){
	  CvPoint point = *point_iter;
	  if(pointInEllipse(point,largestEllipse)){
	    //cvCircle( ellipse_image, point, 1, colors[2], 1, 8, 0 );
	  }
	  else{
	    //cvCircle( ellipse_image, point, 1, colors[4], 1, 8, 0 );
	    left_points.push_back(point);
	  }
	}
	running_time=0;
	largest_count=0;
	if(left_points.size() > 10){
	  do {
	    running_time++;
	    if(running_time > 500)
	      break;
	    
	    Ellipse ellipse = fitEllipseRANSAC(left_points,count);
	    
	    if(count > largest_count){
	      largest_count = count;
	      largestEllipse = ellipse;
	    }
	  }
	  while(largest_count < (.80 * points.size()));
	  if(largestEllipse.majorAxis > 0 && largestEllipse.minorAxis > 0){
	    cvEllipse(imgContourWithPoints,cvPoint(largestEllipse.x,largestEllipse.y),
		      cvSize(largestEllipse.majorAxis,largestEllipse.minorAxis),
		      largestEllipse.angle,0,360,colors[6],2);
	  }
	}
	*/
      }
    }
  }

  cvNamedWindow( "ContourWithPoints", 1 );
  cvMoveWindow( "ContourWithPoints",640,0);
  cvShowImage( "ContourWithPoints", imgContourWithPoints );
  char tmp[100];
  sprintf(tmp,"output/ContourWithPoints%d.png",id);
  cvSaveImage(tmp, imgContourWithPoints);


  // Overlap part    
  //vtrackStore: vector keep all blobs from the previous frames
  //vtrackThis: vector keep all blobs in the current frame
  //vii: iterator of vtrackStore
  //ii: iterator of vcomp (connected comp) of the current frame
  //trackTemp: track tempolary variable
    
  //(xi1, yi1): coordinate of upper left corner of blob in frame1
  //(xi2, yi2): coordinate of bottom right corner of blob in frame1
    
  //(xj1, yj1): coordinate of upper left corner of blob in frame2
  //(xj2, yj2): coordinate of bottom right corner of blob in frame2
    
  //(xk1, yk1): coordinate of upper left corner of overlap part
  //(xk2, yk2): coordinate of bottom right corner of overlap part
  //if((xk1 < xk2) && (yk1 < yk2)): 2 blobs are overlap 
    
  vector<track> vtrackThis;  
  //vector<track>::reverse_iterator vii;
  vector<track>::iterator vii;
  vector<CvConnectedComp>::iterator ii;
  track trackTemp;
    
  // txt file for Ice's defence
  FILE *outFileDetail;
  FILE *outFileID;

  outFileDetail = fopen(outFileDetailName, "w");
	    
  int xi1, yi1, xi2, yi2, xj1, yj1, xj2, yj2, xk1, yk1, xk2, yk2;
  int found;
  if((*vtrackStore).size() == 0){	//start the first frame, store the blobs of the first frame
    //i = 0;		
    for(ii = vcomp.begin(); ii != vcomp.end(); ii++){
      //i = i+1;
      //trackTemp.id = i;
      maxID = maxID+1;
      trackTemp.id = maxID;
      trackTemp.comp = *ii;
      (*vtrackStore).push_back(trackTemp);
      vtrackThis.push_back(trackTemp); 
    		
      fprintf(outFileDetail, "%d ", maxID);
      sprintf(outFileIDName2, "%s%d", outFileIDName1, maxID);
      outFileID = fopen(outFileIDName2, "w");
      fprintf(outFileID, "%d %d %d %d", trackTemp.comp.rect.x, trackTemp.comp.rect.y, trackTemp.comp.rect.width, trackTemp.comp.rect.height);
      fclose(outFileID);
    }    	
  }
  else{		//the second frame and so on
    for(vii = (*vtrackStore).begin(); vii != (*vtrackStore).end(); vii++){
      if((*vii).comp.rect.width != 0) {		//if not NULL still track this blob otherwise do not track any more				
	xi1 = (*vii).comp.rect.x;
	yi1 = (*vii).comp.rect.y;
	xi2 = (*vii).comp.rect.x + (*vii).comp.rect.width;
	yi2 = (*vii).comp.rect.y + (*vii).comp.rect.height;
					
	found = 0;
	for(ii = vcomp.begin(); ii != vcomp.end(); ii++){
	  xj1 = (*ii).rect.x;
	  yj1 = (*ii).rect.y;
	  xj2 = (*ii).rect.x + (*ii).rect.width;
	  yj2 = (*ii).rect.y + (*ii).rect.height;			
				
	  if(xi1 >= xj1){ xk1 = xi1; }
	  else { xk1 = xj1; }				
	  if(yi1 >= yj1) { yk1 = yi1; }
	  else { yk1 = yj1; }

	  if(xi2 <= xj2){ xk2 = xi2; }
	  else { xk2 = xj2; }				
	  if(yi2 <= yj2) { yk2 = yi2; }
	  else { yk2 = yj2; }
				
	  if((xk1 < xk2) && (yk1 < yk2)){  // if found overlap blob keep the last comp and erase from vcomp
	    (*vii).comp = (*ii);
	    vtrackThis.push_back(*vii);
			
	    fprintf(outFileDetail, "%d ", (*vii).id);
	    sprintf(outFileIDName2, "%s%d", outFileIDName1, (*vii).id);
	    outFileID = fopen(outFileIDName2, "w");
	    fprintf(outFileID, "%d %d %d %d", (*vii).comp.rect.x, (*vii).comp.rect.y, (*vii).comp.rect.width, (*vii).comp.rect.height);
	    fclose(outFileID);
						
	    vcomp.erase(ii);
	    found = 1;
	    break;
	  }
	}	
							
	if(found == 0){  	//if not found overlap blob, stop tracking by set the comp.rect.width to 0
	  //cout << "NOT found any blob" << endl;			
	  (*vii).id = NULL;
	  (*vii).comp.rect.width = NULL;
	  (*vii).comp.rect.height = NULL;
	  (*vii).comp.rect.x = NULL;
	  (*vii).comp.rect.y = NULL;
	  //(*vtrackStore).erase(*vii);

	  //cout << "(*vii).comp " << (*vii).comp << endl;
	}

	//cout << "vcomp.size(): " << vcomp.size() << endl;
				
      }	    	
    }// End if((*vii).comp.rect.width != 0)
 		
    for(ii = vcomp.begin(); ii != vcomp.end(); ii++){		//keep the rest as new blobs to vtrackStore
      //trackTemp.id = ((*vtrackStore).size()+1);
      maxID = maxID+1;
      trackTemp.id = maxID;
      trackTemp.comp = *ii;
      (*vtrackStore).push_back(trackTemp);
      vtrackThis.push_back(trackTemp);    		
   			
      fprintf(outFileDetail, "%d ", maxID);
      sprintf(outFileIDName2, "%s%d", outFileIDName1, maxID);
      outFileID = fopen(outFileIDName2, "w");
      fprintf(outFileID, "%d %d %d %d", trackTemp.comp.rect.x, trackTemp.comp.rect.y, trackTemp.comp.rect.width, trackTemp.comp.rect.height);
      fclose(outFileID);
    }
 		
  }  // else{		//the second frame and so on
  fclose(outFileDetail);    

  CvFont font;
  cvInitFont( &font, CV_FONT_HERSHEY_COMPLEX,1.0, 1.0, 0, 1, CV_AA);

  //int x=54500;
  char string[32];
	

  //for(vii = vtrackThis.rbegin(); vii != vtrackThis.rend(); vii++){
  for(vii = vtrackThis.begin(); vii != vtrackThis.end(); vii++){
    sprintf(string,"%d",(*vii).id);
    cvRectangle(colorImage, cvPoint((*vii).comp.rect.x,(*vii).comp.rect.y), cvPoint(((*vii).comp.rect.x + (*vii).comp.rect.width),((*vii).comp.rect.y + (*vii).comp.rect.height)), CV_RGB(255,0,0), 2);
    cvPutText(colorImage, string, cvPoint((*vii).comp.rect.x,(*vii).comp.rect.y), &font, CV_RGB(255, 0, 0) );
  }

  vtrackThis.clear();
  vcomp.clear();
  //cvRelease(&vtrackThis);	
  //time1 = (double)cvGetTickCount() - time1;
  //cout << endl << "---- Time Interest: " << time1/(cvGetTickFrequency()*1000.) << " ms" << endl << endl;

  //cvNamedWindow( "Blob Result", 1 );
  //cvMoveWindow( "Blob Result",0,480);
  //cvShowImage( "Blob Result", colorImage );
  //cvSaveImage(saveFileImagName, colorImage);
   
    
  cvReleaseImage( &object);
  cvReleaseImage( &colorImage);
  cvReleaseImage( &pImageSVM );	
  cvReleaseImage( &pImageBlob );
  //cout << endl << "Number of total ID: " << (*vtrackStore).size() << endl;
  return maxID;
}
